/* helloworld.c for TOPPERS/ATK(OSEK) */ 
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void){ /* do nothing */ }

void ecrobot_device_initialize(){

  nxt_motor_set_speed(NXT_PORT_B, 0,1);
  nxt_motor_set_speed(NXT_PORT_C, 0,1);
  

}


void ecrobot_device_terminate(){
  
  nxt_motor_set_speed(NXT_PORT_B, 0,1);
  nxt_motor_set_speed(NXT_PORT_C, 0,1);


}

TASK(move_motor_1){

  nxt_motor_set_speed(NXT_PORT_B, 20,1);
  nxt_motor_set_speed(NXT_PORT_C, 20,1);  
  
  systick_wait_ms(7000);

  nxt_motor_set_speed(NXT_PORT_B, 0,1);
  nxt_motor_set_speed(NXT_PORT_C, 0,1);  

  systick_wait_ms(200);
  
  nxt_motor_set_speed(NXT_PORT_C, -20,1);

  systick_wait_ms(1500);

  nxt_motor_set_speed(NXT_PORT_B, -20, 1);
  nxt_motor_set_speed(NXT_PORT_C, -20, 1);

  systick_wait_ms(2800);

  nxt_motor_set_speed(NXT_PORT_B, 0,1);
  nxt_motor_set_speed(NXT_PORT_C, 0,1);  

  systick_wait_ms(200);
  
  
  nxt_motor_set_speed(NXT_PORT_B, -20, 1);
  nxt_motor_set_speed(NXT_PORT_C,  20, 1);
  

  systick_wait_ms(780);

  nxt_motor_set_speed(NXT_PORT_B, 0, 1);
  nxt_motor_set_speed(NXT_PORT_C, 0, 1);

  systick_wait_ms(200);
  
  
  nxt_motor_set_speed(NXT_PORT_B,  20, 1);
  nxt_motor_set_speed(NXT_PORT_C,  20, 1);
  

  systick_wait_ms(400);

  nxt_motor_set_speed(NXT_PORT_B, 0, 1);
  nxt_motor_set_speed(NXT_PORT_C, 0, 1);

  TerminateTask();
  
  

 
  
 

}

